/*
 * Team D VEX auxiliary autonomous code.
 * @author Derek Leung, Michael Searing
 */

//////////////////////////
/////// Auto Drive ///////
//////////////////////////
/**
 * Autonomous drive loop.
 * @param direction -1 for backwards or 1 for forwards
 * @param distance in inches
 * @param power max motor power
 * @param turn positive -> clockwise, negative -> counterclockwse.
 * Magnitude is width of turn; 1 yields point turn while 1/2 yields turn with inside wheel immobile.
 */
void regulate_drive(int direction, int distance, int max_power, float turn)
{
  //Declaration
  int drive_target;
  float lturn;
  float rturn;
  float pturn;
  float left_dist;
  float right_dist;
  float correction;
  int drive_distance;
  int drive_err;
  int drive_errprev;
  int drive_ppower;
  int drive_dpower;
  int drive_power;
  //End declaration

	SensorValue(lshaft) = 0;
	SensorValue(rshaft) = 0;
	drive_target = 360 * 1/(PI * 2.8) * distance; //2.8 is wheel diameter
	lturn = 1 + 2*turn;
	if(turn > 0)
	  lturn = 1;
	rturn = 1 - 2*turn;
	if(turn < 0)
	  rturn = 1;
  drive_errprev=0;
	while(drive_target - (abs(SensorValue(lshaft)) + abs(SensorValue(rshaft)))/2 > DRIVE_APPROACH_ERR)
	{
		left_dist = -SensorValue(lshaft);
		right_dist = -SensorValue(rshaft);
		drive_distance = (abs(left_dist) + abs(right_dist))/2;
		drive_err = drive_target - drive_distance;
		if(abs(drive_err) > DRIVE_TURN_ERR && turn == 0)
	  	correction = DRIVE_CORRECTION*(left_dist - right_dist);
	  else
	    correction = 0;
	  pturn = 1 + abs(turn);
    drive_ppower = DRIVE_PMULT*drive_err*pturn;
    drive_dpower = DRIVE_DMULT*(drive_err - drive_errprev);
    drive_power = drive_ppower + drive_dpower;
    if(drive_power > max_power)
      drive_power = max_power;
    if(drive_power < -max_power)
      drive_power = -max_power;
    if(!debug)
    {
	    motor[leftDB] = -direction * (drive_power - correction) * lturn;
	    motor[rightDB] = -direction * (drive_power + correction) * rturn;
	  }
    drive_errprev = drive_err;
    wait1Msec(15);
	}
}

/**
 * Set both motors on drive base to given power.
 */
void set_drivepower(int spower)
{
  if(!debug)
  {
	  motor[leftDB] = spower * -1;
	  motor[rightDB] = spower * -1;
	}
}

////////////////////////
/////// Auto Arm ///////
////////////////////////

/**
 * Set intake to given power.
 */
void set_intake(int ipower)
{
  if(!debug)
    motor[intake] = ipower;
}

/**
 * Autonomous arm loop with a given target.
 */
void regulate_arm(int arm_target)
{
  int real_pot;
  float arm_err;
  float arm_errprev;
  float arm_errtot=0;
  int arm_power;
  int arm_ppower;
  int arm_ipower;
  int arm_dpower;

  if(auton) arm_move = true;
  while(arm_move && (vexRT[Btn7UXmtr2] == 1 || vexRT[Btn7LXmtr2] == 1 || vexRT[Btn7RXmtr2] == 1 || vexRT[Btn7DXmtr2] == 1 || auton))
  {
    if(abs(SensorValue(pot) - arm_target) > ARM_MAXERR)
    {
      wait1Msec(50);
      if(abs(real_pot - SensorValue(pot)) > 2) //Prevents pot fluctuations from causing unwanted dpower
        real_pot = SensorValue(pot);
      arm_err = arm_target - real_pot;
      if(abs(arm_err - arm_errprev) <= 5) //Enable I when moving slowly
      {
        arm_errtot = arm_errtot + sgn(arm_err);
        arm_ipower = ARM_IMULT*arm_errtot;
      }
      else
        arm_ipower = 0;
      arm_ppower = ARM_PMULT*arm_err;
      int arm_change = arm_err - arm_errprev;
      arm_dpower = ARM_DMULT*(arm_err - arm_errprev);
      arm_power = -(arm_ppower + arm_ipower + arm_dpower);
      arm_errprev = arm_err;
    }
    else if(auton)
    {
      arm_power = -12 - 3*objects;
      arm_errtot = 0;
      arm_move = false;
    }
    else
    {
      arm_power = -12;
      arm_errtot = 0;
      arm_move = false;
    }
    if(!debug)
    {
      motor[port1] = -arm_power;
      motor[port3] = arm_power;
      motor[port8] = -arm_power;
      motor[port10]= arm_power;
    }
  }
}

/**
 * Sets arm motors to a given power, correctly (adjusting for polarity).
 */
void set_armpower(int apower)
{
  if(!debug)
	{
	  motor[port1] = apower;
	  motor[port3] = apower * -1;
	  motor[port8] = apower;
	  motor[port10] = apower * -1;
	}
}


//////////////////////////////
/////// Operator Input ///////
//////////////////////////////
//Checking for operator button presses
task oparm()
{
  while(true)
  {
    if(vexRT[Btn7UXmtr2] == 1)
    {
      arm_move = true;
     	regulate_arm(ARM_30);
    }
    else if(vexRT[Btn7LXmtr2] == 1)
    {
      arm_move = true;
    	regulate_arm(ARM_20);
    }
    else if(vexRT[Btn7RXmtr2] == 1)
    {
      arm_move = true;
	    regulate_arm(ARM_10);
    }
    else if(vexRT[Btn7DXmtr2] == 1)
    {
      arm_move = true;
      regulate_arm(ARM_6);
    }
    else
    {
      arm_move = false;
      intake_move = false;
      float arm_errprev = 0;
      wait1Msec(50);
    }
  }
}
